/*
 * Copyright (c) 2011-2025, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/main/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include "dart/dynamics/LineSegmentShape.hpp"

#include "dart/common/Logging.hpp"
#include "dart/math/Geometry.hpp"

namespace dart {
namespace dynamics {

//==============================================================================
const Eigen::Vector3d LineSegmentShape::mDummyVertex = Eigen::Vector3d::Zero();

//==============================================================================
LineSegmentShape::LineSegmentShape(float _thickness)
  : Shape(LINE_SEGMENT), mThickness(_thickness)
{
  if (_thickness <= 0.0f) {
    DART_WARN(
        "Attempting to set non-positive thickness. We set the thickness to "
        "1.0f instead.");
    mThickness = 1.0f;
  }

  mVariance = DYNAMIC_VERTICES;
}

//==============================================================================
LineSegmentShape::LineSegmentShape(
    const Eigen::Vector3d& _v1, const Eigen::Vector3d& _v2, float _thickness)
  : Shape(), mThickness(_thickness)
{
  if (_thickness <= 0.0f) {
    DART_WARN(
        "Attempting to set non-positive thickness. We set the thickness to "
        "1.0f instead.");
    mThickness = 1.0f;
  }

  addVertex(_v1);
  addVertex(_v2);
  mVariance = DYNAMIC_VERTICES;
}

//==============================================================================
const std::string& LineSegmentShape::getType() const
{
  return getStaticType();
}

//==============================================================================
const std::string& LineSegmentShape::getStaticType()
{
  static const std::string type("LineSegmentShape");
  return type;
}

//==============================================================================
void LineSegmentShape::setThickness(float _thickness)
{
  if (_thickness <= 0.0f) {
    DART_WARN(
        "Attempting to set non-positive thickness. We set the thickness to "
        "1.0f instead.");
    mThickness = 1.0f;
    return;
  }

  mThickness = _thickness;
}

//==============================================================================
float LineSegmentShape::getThickness() const
{
  return mThickness;
}

//==============================================================================
std::size_t LineSegmentShape::addVertex(const Eigen::Vector3d& _v)
{
  std::size_t parent = mVertices.size();
  if (parent > 0)
    return addVertex(_v, parent - 1);

  mVertices.push_back(_v);
  return 0;
}

//==============================================================================
std::size_t LineSegmentShape::addVertex(
    const Eigen::Vector3d& _v, std::size_t _parent)
{
  std::size_t index = mVertices.size();
  mVertices.push_back(_v);

  if (_parent > mVertices.size()) {
    if (mVertices.size() == 0)
      DART_WARN(
          "Attempting to add a vertex to be a child of vertex #{}, but no "
          "vertices exist yet. No connection will be created for the new "
          "vertex yet.",
          _parent);
    else
      DART_WARN(
          "Attempting to add a vertex to be a child of vertex #{}, but the "
          "vertex indices only go up to {}. No connection will be created for "
          "the new vertex yet.",
          _parent,
          mVertices.size() - 1);
  } else {
    mConnections.push_back(Eigen::Vector2i(_parent, index));
  }

  return index;
}

//==============================================================================
void LineSegmentShape::removeVertex(std::size_t _idx)
{
  if (_idx >= mVertices.size()) {
    if (mVertices.size() == 0)
      DART_WARN(
          "Attempting to remove vertex #{}, but this LineSegmentShape contains "
          "no vertices. No vertex will be removed.",
          _idx);
    else
      DART_WARN(
          "Attempting to remove vertex #{}, but vertex indices only go up to "
          "#{}. No vertex will be removed.",
          _idx,
          mVertices.size() - 1);

    return;
  }

  mVertices.erase(mVertices.begin() + _idx);
}

//==============================================================================
void LineSegmentShape::setVertex(std::size_t _idx, const Eigen::Vector3d& _v)
{
  if (_idx >= mVertices.size()) {
    if (mVertices.size() == 0)
      DART_WARN(
          "Attempting to set vertex #{}, but no vertices exist in this "
          "LineSegmentShape yet.",
          _idx);
    else
      DART_WARN(
          "Attempting to set vertex #{}, but the vertices of this "
          "LineSegmentShape only go up to #{}.",
          _idx,
          mVertices.size() - 1);

    return;
  }
  mVertices[_idx] = _v;
}

//==============================================================================
const Eigen::Vector3d& LineSegmentShape::getVertex(std::size_t _idx) const
{
  if (_idx < mVertices.size())
    return mVertices[_idx];

  if (mVertices.empty()) {
    DART_WARN(
        "Requested vertex #{}, but no vertices currently exist in this "
        "LineSegmentShape",
        _idx);
  } else {
    DART_WARN(
        "Requested vertex #{}, but vertex indices currently only go up to {}",
        _idx,
        mVertices.size() - 1);
  }

  return mDummyVertex;
}

//==============================================================================
const std::vector<Eigen::Vector3d>& LineSegmentShape::getVertices() const
{
  return mVertices;
}

//==============================================================================
void LineSegmentShape::addConnection(std::size_t _idx1, std::size_t _idx2)
{
  if (_idx1 >= mVertices.size() || _idx2 >= mVertices.size()) {
    if (mVertices.size() == 0)
      DART_WARN(
          "Attempted to create a connection between vertex #{} and vertex #{}, "
          "but no vertices exist for this LineSegmentShape yet. No connection "
          "will be made for these non-existent vertices.",
          _idx1,
          _idx2);
    else
      DART_WARN(
          "Attempted to create a connection between vertex #{} and vertex #{}, "
          "but the vertices only go up to #{}. No connection will be made for "
          "these non-existent vertices.",
          _idx1,
          _idx2,
          mVertices.size());

    return;
  }

  mConnections.push_back(Eigen::Vector2i(_idx1, _idx2));
}

//==============================================================================
void LineSegmentShape::removeConnection(
    std::size_t _vertexIdx1, std::size_t _vertexIdx2)
{
  // Search through all connections to remove any that match the request
  common::aligned_vector<Eigen::Vector2i>::iterator it = mConnections.begin();
  while (it != mConnections.end()) {
    const Eigen::Vector2i c = (*it);
    if ((c[0] == (int)_vertexIdx1 && c[1] == (int)_vertexIdx2)
        || (c[0] == (int)_vertexIdx2 && c[1] == (int)_vertexIdx1)) {
      // Erase this iterator, but not before stepping it forward to the next
      // iterator in the sequence.
      mConnections.erase(it++);
    } else
      ++it;
  }
}

//==============================================================================
void LineSegmentShape::removeConnection(std::size_t _connectionIdx)
{
  if (_connectionIdx >= mConnections.size()) {
    if (mConnections.size() == 0)

      DART_WARN(
          "Attempting to remove connection #{}, but no connections exist yet. "
          "No connection will be removed.",
          _connectionIdx);
    else

      DART_WARN(
          "Attempting to remove connection #{}, but connection indices only go "
          "up to #{}. No connection will be removed.",
          _connectionIdx,
          mConnections.size() - 1);

    return;
  }

  mConnections.erase(mConnections.begin() + _connectionIdx);
}

//==============================================================================
const common::aligned_vector<Eigen::Vector2i>&
LineSegmentShape::getConnections() const
{
  return mConnections;
}

//==============================================================================
Eigen::Matrix3d LineSegmentShape::computeInertia(double _mass) const
{
  Eigen::Matrix3d inertia = Eigen::Matrix3d::Zero();

  double totalLength = 0;
  for (const Eigen::Vector2i& c : mConnections) {
    const Eigen::Vector3d& v0 = mVertices[c[0]];
    const Eigen::Vector3d& v1 = mVertices[c[1]];

    totalLength += (v1 - v0).norm();
  }

  for (const Eigen::Vector2i& c : mConnections) {
    const Eigen::Vector3d& v0 = mVertices[c[0]];
    const Eigen::Vector3d& v1 = mVertices[c[1]];

    double radius = 1e-6;
    double height = (v1 - v0).norm();

    double partialMass = _mass * height / totalLength;

    Eigen::Matrix3d partialInertia = Eigen::Matrix3d::Zero();
    partialInertia(0, 0)
        = partialMass * (3.0 * radius * radius + height * height) / 12.0;
    partialInertia(1, 1) = partialInertia(0, 0);
    partialInertia(2, 2) = 0.5 * _mass * radius * radius;

    Eigen::Vector3d v = v1 - v0;
    Eigen::Vector3d axis;
    double angle;
    if (v.norm() == 0) {
      angle = 0;
      axis = Eigen::Vector3d::UnitX();
    } else {
      v.normalize();
      axis = Eigen::Vector3d::UnitZ().cross(v);
      if (axis.norm() == 0) {
        angle = 0;
        axis = Eigen::Vector3d::UnitX();
      } else {
        axis.normalize();
        angle = acos(Eigen::Vector3d::UnitZ().dot(v));
      }
    }

    Eigen::AngleAxisd R(angle, axis);
    Eigen::Vector3d center = (v1 + v0) / 2.0;
    inertia += R.matrix()
               * math::parallelAxisTheorem(partialInertia, center, partialMass)
               * R.matrix().transpose();
  }

  return inertia;
}

//==============================================================================
ShapePtr LineSegmentShape::clone() const
{
  auto new_shape = std::make_shared<LineSegmentShape>(mThickness);
  new_shape->mVertices = mVertices;
  new_shape->mConnections = mConnections;

  return new_shape;
}

//==============================================================================
void LineSegmentShape::updateBoundingBox() const
{
  if (mVertices.empty()) {
    mBoundingBox.setMin(Eigen::Vector3d::Zero());
    mBoundingBox.setMax(Eigen::Vector3d::Zero());
    mIsBoundingBoxDirty = false;
    return;
  }

  Eigen::Vector3d min
      = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity());
  Eigen::Vector3d max
      = Eigen::Vector3d::Constant(-std::numeric_limits<double>::infinity());

  for (const auto& vertex : mVertices) {
    min = min.cwiseMin(vertex);
    max = max.cwiseMax(vertex);
  }

  mBoundingBox.setMin(min);
  mBoundingBox.setMax(max);

  mIsBoundingBoxDirty = false;
}

//==============================================================================
void LineSegmentShape::updateVolume() const
{
  mVolume = 0.0;
  mIsVolumeDirty = false;
}

} // namespace dynamics
} // namespace dart
